#region Using Statements
using System;
using System.Collections.Generic;
using Microsoft.Xna.Framework;
using Microsoft.Xna.Framework.Graphics;
#endregion

namespace KLib.Physics.Collisions
{
#if !XBOX
    [System.ComponentModel.TypeConverter(typeof(CustomPropery<Geom>))]
#endif
    public class Geom : IPositionable, IRotatable
    {

        #region Fields
        private Vector2 offsetPosition;
        internal Vector2 position;

        private float rotation;

        private Vertices localVerts;
        private Vertices worldVerts;

        private RigidBody body;
        private Grid grid;

        private Matrix matrix = Matrix.Identity;
        private Matrix inverseMatrix = Matrix.Identity;
        private AABB bounds = new AABB();

        private bool collisionResponseEnabled = true;

        private float restitutionCoefficient;
        #endregion

        #region Properties
        /// <summary>
        /// Gets the Position of the Geometry. To set the position use Move() or MoveTo()
        /// </summary>
        public Vector2 Position
        {
            get { return position; }
        }

        public Vector2 OffsetPosition
        {
            get { return offsetPosition; }
        }

        /// <summary>
        /// Gets the Rotation of the Geometry. To set the rotation use Rotate() or RotateTo()
        /// </summary>
        public float Rotation
        {
            get { return rotation; }
        }

        /// <summary>
        /// Gets the Geometry's Vertices in Object space.
        /// </summary>
        public Vertices LocalVertices
        {
            get { return localVerts; }
            set { localVerts = value; }
        }

        /// <summary>
        /// Gets the Geometry's Vertices in World space.
        /// </summary>
        public Vertices WorldVertices
        {
            get
            {
                /*matrix = Matrix.CreateRotationZ(rotation) * Matrix.CreateTranslation(new Vector3(position, 0));
                for (int i = 0; i < worldVerts.Count; i++)
                {
                    worldVerts[i] = Vector2.Transform(localVerts[i], matrix);
                    //worldVerts[i] = Vector2.Add(worldVerts[i],position);
                }*/
                return worldVerts;
            }
        }

        public Grid Grid
        {
            get { return grid; }
            set { grid = value; }
        }

        public RigidBody Body
        {
            get { return body; }
            set { body = value; }
        }
        /// <summary>
        /// Gets a Matrix that describes the orientation of the Geometry.
        /// </summary>
        public Matrix Matrix
        {
            get { return matrix; }
            internal set
            { 
                matrix = value;
                inverseMatrix = Matrix.Invert(matrix);
            }
        }

        /// <summary>
        /// Gets an Axis-Aligned Bounding Box of the Geometry
        /// </summary>
        public AABB Bounds
        {
            get 
            {
                return bounds; 
            }
        }

        public float RestitutionCoefficient
        {
            get { return restitutionCoefficient; }
            set { restitutionCoefficient = value; }
        }
        public bool CollisionResponseEnabled
        {
            get { return collisionResponseEnabled; }
            set { collisionResponseEnabled = value; }
        }
        #endregion

        #region Initialization

        public Geom(RigidBody body)
        {
            this.body = body;
            localVerts = new Vertices();
            worldVerts = new Vertices();
            grid = new Grid();
        }

        public Geom(RigidBody body, Vertices vertices)
        {
            this.body = body;
            grid = new Grid();
            SetVertices(vertices);
        }

        #endregion

        #region Methods
        public void SetVertices(Vertices vertices)
        {
            localVerts = new Vertices(vertices);
            worldVerts = new Vertices(vertices);
            grid.CreateGrid(this, 12.8f);
            bounds.Update(WorldVertices);
        }

        //This method COULD be the big problem... but I doubt it
        public void Update()
        {
            position = Vector2.Transform(offsetPosition, Matrix.CreateTranslation(new Vector3(body.Position,0)));
            //position += offsetPosition;
            /*Vector2 diffVect =body.Position - offsetPosition;

            position = Vector2.Transform(diffVect,Matrix.CreateRotationZ(body.Rotation));
            position += offsetPosition;*/

            position.X = (float)Math.Cos(body.Rotation) * offsetPosition.X - (float)Math.Sin(body.Rotation) * offsetPosition.Y;
            position.Y = (float)Math.Sin(body.Rotation) * offsetPosition.X + (float)Math.Cos(body.Rotation) * offsetPosition.Y;

            position += body.position;

            matrix = Matrix.CreateRotationZ(body.Rotation) * Matrix.CreateRotationZ(rotation) *  Matrix.CreateTranslation(new Vector3(position, 0));
            inverseMatrix = Matrix.Invert(matrix);
            //matrix = Matrix.CreateRotationZ(body.Rotation) * Matrix.CreateTranslation(new Vector3(position, 0)) * Matrix.Invert(Matrix.CreateRotationZ(body.Rotation));


            for (int i = 0; i < worldVerts.Count; i++)
            {
                worldVerts[i] = Vector2.Transform(localVerts[i], matrix);
                //worldVerts[i] = GetWorldCoordinates(localVerts[i]);
            }
            bounds.Update(worldVerts);
        }

        public float GetNearestDistance(Vector2 vector)
        {
            float distance;
            Feature nearestFeature = new Feature(vector);
            nearestFeature.Distance = float.MaxValue;

            for (int i = 0; i < localVerts.Count; i++)
            {
                Feature feature = GetNearestFeature(vector, i);
                if (feature.Distance < nearestFeature.Distance)
                    nearestFeature = feature;
            }
            Vector2 diff = Vector2.Subtract(vector, nearestFeature.Position);
            float dot = Vector2.Dot(diff, nearestFeature.Normal);

            if (dot < 0)
                distance = -nearestFeature.Distance;
            else
                distance = nearestFeature.Distance;

            return distance;
        }

        public Feature GetNearestFeature(Vector2 point, int index)
        {
            Feature feature = new Feature();
            Vector2 v = localVerts.GetEdge(index);
            Vector2 w = Vector2.Subtract(point, localVerts[index]);

            float c1 = Vector2.Dot(w, v);
            if (c1 < 0)
            {
                feature.Position = localVerts[index];
                feature.Normal = localVerts.GetVertexNormal(index);
                feature.Distance = Math.Abs(w.Length());

                return feature;
            }

            float c2 = Vector2.Dot(v, v);
            if (c2 <= c1)
            {
                Vector2 d1 = Vector2.Subtract(point,LocalVertices[localVerts.NextIndex(index)]);
                feature.Position = localVerts[localVerts.NextIndex(index)];
                feature.Normal = localVerts.GetVertexNormal(localVerts.NextIndex(index));
                feature.Distance = Math.Abs(d1.Length());
                return feature;
            }

            float b = c1 / c2;
            v = Vector2.Multiply(v, b);
            Vector2 pb = Vector2.Add(localVerts[index], v);
            Vector2 d2 = Vector2.Subtract(point, pb);

            feature.Position = pb;
            feature.Normal = localVerts.GetEdgeNormal(index);
            feature.Distance = d2.Length();
            return feature;
        }

        public void TransformToLocalCoordinates(ref Vector2 worldVertex, out Vector2 localVertex)
        {
            Vector2.Transform(ref worldVertex, ref inverseMatrix, out localVertex);
        }

        public void TransformNormalToWorld(ref Vector2 localNormal, out Vector2 worldNormal)
        {
            Vector2.TransformNormal(ref localNormal, ref matrix, out worldNormal);

        }

        public Vector2 GetWorldCoordinates(Vector2 vector)
        {
            return Vector2.Transform(vector, matrix);
        }
        #endregion

        /*public bool Collide(Vector2 vert)
        {
            Feature feature;
            vert = Vector2.Transform(vert, this.inverseMatrix);
            
        }*/

        #region Helper Methods
        private int NextIndex(int index)
        {
            if (index >= localVerts.Count - 1)
                return 0;
            else
                return ++index;
        }

        private int PreviousIndex(int index)
        {
            if (index <= 0)
                return localVerts.Count - 1;
            else
                return --index;
        }
        #endregion

        #region IPositionable Members
        public void SetGeometryOffset(Vector2 offset)
        {
            this.offsetPosition = offset;
        }
        public void Move(Vector2 amount)
        {
            //this.position.X += amount.X;
            //this.position.Y += amount.Y;

            //this.offsetPosition.X += amount.X;
            //this.offsetPosition.Y += amount.Y;
            body.Move(amount);
        }

        public void MoveTo(Vector2 position)
        {
            //this.position = position;
            //this.offsetPosition = position;
        }

        #endregion

        #region IRotatable Members

        public void Rotate(float radians)
        {
            this.rotation += radians;
            body.Rotate(radians);
        }

        public void RotateTo(float radians)
        {
            //this.rotation = radians;
            body.RotateTo(radians);
        }

        #endregion
    }
}
